#ifndef  CONTROLLER_NODE_H
#define CONTROLLER_NODE_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <thread>
#include <functional>
#include <mutex>

namespace controller
{

class ControllerNode
{
private:

    ros::NodeHandle nh;
    std::thread callback_thread;
    std::unique_ptr<ros::AsyncSpinner> spinner;  // 使用智能指针管理
    ros::Rate rate;

    ros::Subscriber state_sub;
    mavros_msgs::State current_state;

    ros::Publisher local_pos_pub;
    geometry_msgs::PoseStamped pose;

    ros::ServiceClient arming_client;
    mavros_msgs::CommandBool arm_cmd;

    ros::ServiceClient set_mode_client;
    mavros_msgs::SetMode set_mode;


    void callback_handler(void);
    void state_callback(const mavros_msgs::State::ConstPtr& msg);
    
public:

    ControllerNode(int argc, char **argv);
    ~ControllerNode();

    void arm_control(bool arm);
    void mode_control(std::string mode);
    void position_control(float x, float y, float z);

};



}

#endif